/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-11-03 14:53:38
 * @LastEditTime: 2021-11-06 13:41:05
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/models/meshing/polar_meshing.hpp"

#include <yaml-cpp/yaml.h>

namespace lidar_slam {

PolarMeshing::PolarMeshing(const YAML::Node& config_node) {
	float res_distance = config_node["res_distance"].as<float>();
	float res_angle = config_node["res_angle"].as<float>();
	res_distance_ = res_distance;
	res_angle_ = res_angle;
}

PolarMeshing::PolarMeshing(float res_distance, float res_angle) 
	: res_distance_(res_distance), res_angle_(res_angle){
}

/**
 * @description:  将点云转化为all_mesh_data_数据，也是该类的具体实现
 * @param  {*} 这里的input_cloud_ptr似乎没有分配内存
 * @return {*}
 */
bool PolarMeshing::CloudToMesh(const CloudData::CLOUD_PTR& input_cloud_ptr) {
    int row_i, col_j, meshID;
	float x, y, r, a;
	MeshData mesh_data;
    for (size_t i = 0; i < input_cloud_ptr->size(); ++i) {
		x = input_cloud_ptr->points[i].x;
		y = input_cloud_ptr->points[i].y;
		r = sqrt(x * x + y * y);
		a = atan2(y, x) * 180 / M_PI;
		row_i = floor(r / res_distance_);
        col_j = floor(a / res_angle_);
		
        meshID = row_i * Width() + col_j;
        auto ite = all_mesh_data_.find(meshID);
        if (ite == all_mesh_data_.end()) {
            mesh_data.PtID.insert(i);
            mesh_data.zMinPtID = i;
            all_mesh_data_.insert(std::make_pair(meshID, mesh_data));
            mesh_data.PtID.clear();
        } else {
            ite->second.PtID.insert(i);
            if (input_cloud_ptr->points[i].z <
				input_cloud_ptr->points[ite->second.zMinPtID].z) {
				ite->second.zMinPtID = i;
			}
        }       
    }
	return true;
}




PolarMeshing::~PolarMeshing() {
	all_mesh_data_.clear();
}

/* 这个函数主要就是返回这个值的，all_mesh_data */
bool PolarMeshing::GetAllMeshData(std::map<int, MeshData>& all_mesh_data) {
	all_mesh_data = all_mesh_data_;
	return true;
}


}
